SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
main.cpp:	rt->pid = pid;
main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
main.cpp:		printf("mode %d \n", rt->userMode);
main.cpp:		userExit = rt->userExit;
main.cpp://		printf("exit: %d \n", rt->userExit);
main.cpp:		switch(rt->userMode)
main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
main.cpp:					if(rt->miniMode == TRUE)
main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
main.cpp://					if(rt->miniMode == TRUE)
main.cpp:			rt->q[i] = q[i];
main.cpp:		rt->time = servoTimer.time() - timeStart;
main.cpp:		time = rt->time;
main.cpp:	if(rt->miniMode == TRUE)
main.cpp:	rt->exit_flag = 1;
printer.cpp:		if(kill(rt->pid,0) == -1)
printer.cpp:			// rt->time is time elapsed in control process
printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
printer.cpp:			printf("time:	 %6.1f\n", rt->time);
printer.cpp:			switch(rt->user_mode)
printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
printer.cpp:			//			rt->x[1], rt->x[2]);
printer_org.cpp:		if(kill(rt->pid,0) == -1)
printer_org.cpp:			// rt->time is time elapsed in control process
printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
printer_org.cpp:			switch(rt->user_mode)
printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
printer_org.cpp:			//			rt->x[1], rt->x[2]);
printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
printer_org.cpp:						rt->xee[1], rt->xee[2]);
printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
printer_org.cpp:						57.3*rt->jgoal[2],
printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
printer_org.cpp:						57.3*rt->jgoal[2],
printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
printer_org.cpp:		//				rt->x[1], rt->x[2]);
printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
printer_org.cpp:						rt->xee[1], rt->xee[2]);
printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
printer_org.cpp:						rt->F[1], rt->F[2]);
printer_org.cpp:						'x'+rt->compliance_direction);
printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
printer_org.cpp:			//			rt->x[1], rt->x[2]);
printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
printer_org.cpp:						rt->xee[1], rt->xee[2]);
printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
printer_org.cpp:							rt->desiredForce[2]);
printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
printer_org.cpp:					rt->force[1], rt->force[2]);
printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
printer_org.cpp:						rt->F[1], rt->F[2]);
printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
printer_org.cpp:							rt->desiredForce[2]);
printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
printer_org.cpp:					rt->force[1], rt->force[2]);
printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
printer_org.cpp:						rt->F[1], rt->F[2]);
printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
printer_org.cpp:			//			rt->x[1], rt->x[2]);
printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
printer_org.cpp:						rt->xee[1], rt->xee[2]);
printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
printer_org.cpp:						57.3*rt->jgoal[2],
printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
printer_org.cpp:					rt->force[1], rt->force[2]);
user.cpp:		if(kill(rt->pid,0) == -1)
user.cpp:						rt->userMode = DISPLAY_INFO;
user.cpp:						rt->userMode = IDLE;
user.cpp:						rt->userExit = 1;
user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
user.cpp:							rt->userMode = ENCODER_TEST;
user.cpp:						rt->userMode = FLOAT;
user.cpp:						rt->userMode = INIT_POSITION;
user.cpp:						rt->userMode = JOINT_SERVO;
user.cpp:						rt->xtime = rt->time;
user.cpp:						printf("Time: %f \n", rt->xtime);
user.cpp:						rt->qgoal[0] = tmp[0];
user.cpp:						rt->qgoal[1] = tmp[1];
user.cpp:						rt->qgoal[2] = tmp[2];
user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
user.cpp:						calcSpline(rt->qgoal);
user.cpp:						rt->miniMode = TRUE;
user.cpp:						rt->miniMode = FALSE;
user.cpp:		rt->tf[i] = 0.0;
user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
user.cpp:		if(tf[i]> rt->tf[i])
user.cpp:			rt->tf[i]= tf[i];		
user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
user.cpp:		if(rt->tf[i] > 0.0)
user.cpp:			rt->a0[i] = rt->q[i];
user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
user.cpp:			rt->a0[i] = 0.0;
user.cpp:			rt->a2[i] = 0.0;
user.cpp:			rt->a3[i] = 0.0;
user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:main.cpp:	rt->pid = pid;
userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:main.cpp:		userExit = rt->userExit;
userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:main.cpp:		switch(rt->userMode)
userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:main.cpp:			rt->q[i] = q[i];
userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:main.cpp:		time = rt->time;
userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:main.cpp:	rt->exit_flag = 1;
userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:printer.cpp:			switch(rt->user_mode)
userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:user.cpp:						rt->userMode = IDLE;
userMode:user.cpp:						rt->userExit = 1;
userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:user.cpp:						rt->userMode = FLOAT;
userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:user.cpp:						rt->xtime = rt->time;
userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:user.cpp:						rt->miniMode = TRUE;
userMode:user.cpp:						rt->miniMode = FALSE;
userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.3f %7.3f %7.3f \n",rt->x_des[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->x_des[1],rt->x_des[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//			printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		//				rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.2f %7.2f %7.2f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						'x'+rt->compliance_direction);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("xgoal	: %7.4f %7.4f %7.4f \n",rt->xgoal[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xgoal[1],rt->xgoal[2]); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[0], rt->desiredForce[1],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:							rt->desiredForce[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("Force sensor:  %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("F(N)	: %7.3f %7.3f %7.3f \n",rt->F[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->F[1], rt->F[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[0], 57.3*rt->q[1], 57.3*rt->q[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q[3], 57.3*rt->q[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[0], 57.3*rt->q_des[1], 57.3*rt->q_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->q_des[3], 57.3*rt->q_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[0], 57.3*rt->dq[1], 57.3*rt->dq[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				57.3*rt->dq[3], 57.3*rt->dq[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau[0], rt->tau[1], rt->tau[2], rt->tau[3], rt->tau[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[0], rt->tau_des[1], rt->tau_des[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:				rt->tau_des[3], rt->tau_des[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("Force sensor: %7.3f %7.3f %7.3f \n", rt->force[0],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					rt->force[1], rt->force[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = DISPLAY_INFO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = IDLE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userExit = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("user entered exit: %d, %d \n",rt->userExit,SM->userExit); 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:							rt->userMode = ENCODER_TEST;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = FLOAT;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = INIT_POSITION;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->userMode = JOINT_SERVO;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->xtime = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						printf("Time: %f \n", rt->xtime);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[0] = tmp[0];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[1] = tmp[1];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->qgoal[2] = tmp[2];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->q[0], rt->q[1], rt->q[2], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:									rt->qgoal[0], rt->qgoal[1], rt->qgoal[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						calcSpline(rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = TRUE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:						rt->miniMode = FALSE;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		rt->tf[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		tf[i] = abs((3.0*(qgoal[i] - rt->q[i]))/(dqmax*2.0));
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(tf[i]> rt->tf[i])
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->tf[i]= tf[i];		
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:	printf("Motion to be achieved in: %f, %f, %f \n", rt->tf[0], rt->tf[1], rt->tf[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		if(rt->tf[i] > 0.0)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = rt->q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 3.0*(qgoal[i] - rt->q[i])/(rt->tf[i]*rt->tf[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = -2.0*(qgoal[i]- rt->q[i])/(rt->tf[i]*rt->tf[i]*rt->tf[i]) ;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a0[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a2[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:			rt->a3[i] = 0.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("axis[%d]: qgoal:%7.2f, q: %7.2f; ", i, qgoal[i], rt->q[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:user.cpp:		printf("a0, a2, a3: %f, %f, %f \n", rt->a0[i],rt->a2[i],rt->a3[i]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	rt->ad_voltage[0] = 3.0;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:SharedMemory.cpp:	printf("ad voltage 0: %f \n",rt->ad_voltage[0]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->pid = pid;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	while(!tcischars(1) && (rt->userExit != 1))
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		printf("mode %d \n", rt->userMode);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		userExit = rt->userExit;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://		printf("exit: %d \n", rt->userExit);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		switch(rt->userMode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setSplineParameters(rt->a0,rt->a2,rt->a3,rt->tf, rt->qgoal);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					runtime = time - rt->xtime;	// rt->xtime is the trajectory start time
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:					arm3dof->setLoadTorqueCommand(rt->userMode);			// set load torque command to desired
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp://					if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:			rt->q[i] = q[i];
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		rt->time = servoTimer.time() - timeStart;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:		time = rt->time;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	if(rt->miniMode == TRUE)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:main.cpp:	rt->exit_flag = 1;
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//		printf("x(m)	: %7.4f %7.4f %7.4f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:		if(kill(rt->pid,0) == -1)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->time is time elapsed in control process
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			// rt->honda_count is the servo loop count from Honda controller
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:	//		printf("time:	 %6.1f, %6.1f,  %d\n", rt->time, (time4-time5), rt->honda_count);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			printf("time:	 %6.1f\n", rt->time);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			switch(rt->user_mode)
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//		printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->x[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:			//			rt->x[1], rt->x[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:					printf("x(m)	: %7.3f %7.3f %7.3f \n",rt->xee[0], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						rt->xee[1], rt->xee[2]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[2],
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[3], 57.3*rt->jgoal[4]);
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:printer_org.cpp:						57.3*rt->jgoal[0], 57.3*rt->jgoal[1], 
userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode:userMode: